#include "TinkerToy.h"

TinkerToy::TinkerToy(void)
{
}

TinkerToy::TinkerToy( double timestep, double alpha, double beta, int solverType )
{
	ps=new ParticleSystem();
	cs=new ConstraintSystem(alpha,beta);
	ls=new LinearSystemSolver();
	os=new ODESolver();

	ls->solverType=solverType;

	this->timestep=timestep;

	double pos[2]={0,0};
	double vel[2]={0,0};
	double force[2]={0,0};

	origin=new Particle(pos,vel,force);
	origin->index=-1;
}

TinkerToy::~TinkerToy(void)
{
}

void TinkerToy::AddParticle( double x, double y, bool onCircle, double vx, double vy )
{
	double pos[2]={x,y};
	double vel[2]={vx,vy};
	double force[2]={0,0};

	Particle *p=new Particle(pos,vel,force);
	p->onCircle=onCircle;

	ps->AddParticle(p);
}

void TinkerToy::AddConstraint( int indexA, int indexB )
{
	if (indexA>indexB)
	{
		int swap=indexB;
		indexB=indexA;
		indexA=swap;
	}

	Particle *pA, *pB;

	pB=ps->particles[indexB];

	if (indexA<0)
	{
		pA=origin;
	}else{
		pA=ps->particles[indexA];
	}

	double distance=sqrt((pA->pos[0]-pB->pos[0])*(pA->pos[0]-pB->pos[0])+(pA->pos[1]-pB->pos[1])*(pA->pos[1]-pB->pos[1]));

	Constraint *c=new Constraint(pA,pB,distance);

	cs->AddConstraint(c);
}

void TinkerToy::TinkerInit()
{
	ls->SystemInitialize(ps,cs);
	os->SolverInitialize(timestep,ls,ps);
}

void TinkerToy::TinkerUpdate()
{
	os->Solve();
}

int TinkerToy::GetParticle( double x, double y )
{
	for (int i=0;i<ps->particles.size();i++)
	{
		Particle *p=ps->particles.at(i);

		double rx, ry;
		rx=abs(x-p->pos[0]);
		ry=abs(y-p->pos[1]);

		if (rx<1&&ry<1)
		{
			return p->index;
		}
	}

	return -1;
}
